#include <windows.h>
#include <gl/GL.h>
#include "frame.h"
#include <math.h>

//----------------------------------------------- getLengthError ------------------------------------------------//
float Frame::getLengthError()
{
	return abs(dotProduct(up, up)-1) + abs(dotProduct(forward, forward)-1);
}
// ---------------------------------------------- getPerpendicularError -----------------------------------------//
float Frame::getPerpendicularError()
{
	return abs(dotProduct( up, forward ));
}
//----------------------------------------------- getError ------------------------------------------------------//
float Frame::getError()
{
	return abs(dotProduct(up, up)-1) + abs(dotProduct(forward, forward)-1) + abs(dotProduct( up, forward ));
}
//----------------------------------------------- normalize -----------------------------------------------------//
void Frame::normalize()
{
	/* wykorzystuje ortogonalizacje grama-schmita */

	forward.normalize(); // e1
	up.normalize();      // e2

	up = up - (forward * dotProduct( up, forward ));
	up.normalize();
}
//----------------------------------------------- operator= -----------------------------------------------------//
void Frame::operator=( Frame& t )
{
	origin = t.GetOrigin();
	forward = t.GetForwardVector();
	up = t.GetUpVector();
}
//----------------------------------------------- GetMatrix -----------------------------------------------//
Matrix44< float > Frame::GetMatrix( bool rotationOnly )
{
	//tworzymy macierz
	M44 matrix;

	// Calculate the right side (x) vector, drop it right into the matrix
	V3 xAxis = GetXAxis();

	//Ustawiamy pierwsza kolumne   //X
	matrix.SetMatrixColumn( 1, xAxis.get(1), xAxis.get(2), xAxis.get(3), 0 );

	//Ustawimy druga kolumne  //Y
	matrix.SetMatrixColumn( 2, up.get(1), up.get(2), up.get(3), 0 );
	                    
	//Ttrzecia	//Z
	matrix.SetMatrixColumn( 3, forward.get(1), forward.get(2), forward.get(3), 0 );

	// Translation (already done)
	if( rotationOnly == true )
	{
		matrix[12] = 0.0f;
		matrix[13] = 0.0f;
		matrix[14] = 0.0f;
	}
	else
		matrix.SetMatrixColumn( 4, origin.get(1), origin.get(2), origin.get(3), 1 );

	// skala
	if( !rotationOnly )
	{
		matrix.MultiplyByScaleMatrix( scaleX, scaleY, scaleZ, 1 );
	}

	return matrix;
}
//----------------------------------------------- ApplyActorTransform -------------------------------------//
void Frame::ApplyActorTransform( bool rotationOnly )
{
	M44 rotMat;

	rotMat = GetMatrix( rotationOnly );

	// Apply rotation to the current matrix
	glMultMatrixf( rotMat.GetDataPointer() );
}
//----------------------------------------------- RotateLocalX --------------------------------------------//
void Frame::RotateLocalX( float angle )
{
	M44 rotMat;
	V3 xAxis = GetXAxis();

	rotMat.LoadRotationAboutArbitrary( angle, xAxis.get(1), xAxis.get(2), xAxis.get(3) );

	//jak juz mamy macierz obrotu wymnazamy ja przez wektory up i forward dostajac newUp i newForward
	
	//up
	V3 newUp;
	newUp.set(1, rotMat[0] * up.get(1) + rotMat[4] * up.get(2) + rotMat[8] * up.get(3) );
	newUp.set(2, rotMat[1] * up.get(1) + rotMat[5] * up.get(2) + rotMat[9] * up.get(3) );
	newUp.set(3, rotMat[2] * up.get(1) + rotMat[6] * up.get(2) + rotMat[10] * up.get(3) );

	up = newUp;

	//forward
	V3 newForward;
	newForward.set(1, rotMat[0] * forward.get(1) + rotMat[4] * forward.get(2) + rotMat[8] * forward.get(3) );
	newForward.set(2, rotMat[1] * forward.get(1) + rotMat[5] * forward.get(2) + rotMat[9] * forward.get(3) );
	newForward.set(3, rotMat[2] * forward.get(1) + rotMat[6] * forward.get(2) + rotMat[10] * forward.get(3) );

	forward = newForward;
}
//----------------------------------------------- RotateLocalY --------------------------------------------//
void Frame::RotateLocalY( float angle )
{
	M44 rotMat;
	V3 yAxis = GetYAxis();

	rotMat.LoadRotationAboutArbitrary( angle, yAxis.get(1), yAxis.get(2), yAxis.get(3) );

	//jak juz mamy macierz obrotu wymnazamy ja przez wektor forward dostajac newForward

	//forward
	V3 newForward;
	newForward.set(1, rotMat[0] * forward.get(1) + rotMat[4] * forward.get(2) + rotMat[8] * forward.get(3) );
	newForward.set(2, rotMat[1] * forward.get(1) + rotMat[5] * forward.get(2) + rotMat[9] * forward.get(3) );
	newForward.set(3, rotMat[2] * forward.get(1) + rotMat[6] * forward.get(2) + rotMat[10] * forward.get(3) );

	forward = newForward;
}
//------------------------------------------------- RotateLocalZ ------------------------------------------//
void Frame::RotateLocalZ( float angle )
{
	M44 rotMat;
	V3 zAxis = GetZAxis();

	rotMat.LoadRotationAboutArbitrary( angle, zAxis.get(1), zAxis.get(2), zAxis.get(3) );

	//jak juz mamy macierz obrotu wymnazamy ja przez wektor up dostajac newUp

	//up
	V3 newUp;
	newUp.set(1, rotMat[0] * up.get(1) + rotMat[4] * up.get(2) + rotMat[8] * up.get(3) );
	newUp.set(2, rotMat[1] * up.get(1) + rotMat[5] * up.get(2) + rotMat[9] * up.get(3) );
	newUp.set(3, rotMat[2] * up.get(1) + rotMat[6] * up.get(2) + rotMat[10] * up.get(3) );

	up = newUp;
}